Sensor Fusion and Object Tracking using an Extended Kalman Filter Algorithm — Part 1
An overview of the Kalman Filter algorithm and what the matrices and vectors mean.
🐳 ☕ ️🧧
33Mudy961bUk9zz35p68g9fE3uuHLRduRpKalman Filtering uses imperfect measurements observed over time and produces estimates of unknown variables.
This algorithm is a recursive two-step process: prediction, and update. The prediction step produces estimates of the current variables along with their uncertainties. These estimates are based on the assumed model of how the estimates change over time. The update step is done when the next measurements (subject to noise) is observed. In this step, the estimates (let’s call it state from here on) are updated based on the weighted average of the predicted state and the state based on the current measurement. A lower weight is given to that with a higher uncertainty.
It is interesting to note that the Kalman Filter doesn’t need the whole history of past measurements and states to do its thing, it only uses the present input measurement and the previously calculated state and uncertainty.
All of these seem pretty abstract, but hopefully things get clearer as I proceed with the discussion. :)
For one of the Udacity’s requirements, I implemented an Extended Kalman Filter algorithm to predict the position (px, py) and velocity (vx, vy) of a moving object given somewhat noisy stream of measurements from a lidar sensor, and a radar sensor. The algorithm fuses these measurements. Along with the assumption that the object being tracked is moving at a constant velocity, it produces the state estimate (px, py, vx, vy) that is more accurate than any one of the aforementioned information sources alone.
A lidar can measure distance of a nearby objects that can easily be converted to cartesian coordinates (px, py) . A radar sensor can measure speed within its line of sight (drho)using something called a doppler effect. It can also measure distance of nearby objects that can easily be converted to polar coordinates (rho, phi) but in a lower resolution than lidar.
You can easily convert values from cartestian to polar coordinate systems and vice versa.
If I understand what I’m doing, the Kalman Filter two-step process looks like this in code:
Though concise, it seems somewhat overwhelming having a non-trivial number of vectors and matrices. Here’s a good discussion of all those matrices which helped me understand them a little bit better, I think.
The x vector holds the estimate of the state of the object being tracked which is four values total: the 2D position in cartesian coordinates and the cartesian components of its velocity(px, py, vx, vy).
The process or state covariance matrix P holds the current uncertainty of the states. Since I have n = 4 elements in the state vector x, the P matrix is n x n = 4 x 4. Say if I observe that most of the time, as the magnitude of position py goes higher, the magnitude of velocity vx goes higher as well; this means py and vx have a covariance. But if I observe no pattern between how the state elements move, I can say that they have zero covariance.
The diagonal of P contains the variance of that element with itself, the bigger the numbers the more uncertain I am. Around the diagonals are the covariances between two state elements, which is how the values of the state elements “co-vary” with each other. All covariance matrices such as this one (also R, S, andQ if i’m correct) are symmetric matrices, because the covariance of py and vx is also the covariance of vx and py.
We start off pretty uncertain but as this filter learns — if I’ve implemented this algorithm correctly — we should become more and more certain.
Similarly, z holds the measurements from the sensor, and R is the covariance matrix of these measurements. For a lidar measurement it only holds two (number of sensor measurements at a time m = 2 ), the position (px, py). For a radar measurement it holds three (rho, phi, drho) (number of sensor measurements at a timem = 3) as discussed earlier. Parallel to the discussion above, the R matrix for a lidar is 2 x 2 while the R matrix for a radar is 3 x 3. These measurements are given by the manufacturer of the sensors.
Matrix F is the update matrix (aka state transition matrix) used to predict the value of the next x and P. It’s an n x n = 4 x 4 matrix. To do this I need a model of how the system behaves. Matrix F depends on this model. As mentioned earlier, I’m assuming that the object being tracked is just moving straight at a constant velocity. I use this model to get the updated x given the elapsed time dt .
Check the snippet below.
Matrix Q is the process noise covariance matrix where I add a little uncertainty to this process covariance matrix; given that there are various components in the system such as acceleration noise (ax, ay)that make our system not completely linear. I’ll get back to how Q is computed later.
Not yet discussed are H, y, S, and K.
Matrix H, the extraction matrix, is what is used to extract the theoretical sensor readings Hx, if the predicted state is true and the sensor used is perfect. A perfect lidar measurement (px, py) is actually just the first two elements of a true state (px, py, vx, vy).The extraction matrix has m = 2 rows which is the number of sensor measurements and n = 4 columns which is the number of state elements. Check the demonstration below.
Unfortunately, it’s not as straightforward for the radar sensor since it is in polar coordinates. This makes the extraction non-linear (which is why this project is an extended Kalman filter as opposed to a simple Kalman filter). Something called a Jacobian is used to linearize this non-linearity and estimate H, again I’ll get back to this later.
Matrix S, the innovation covariance somehow combines the covariance of the state P, the covariance of the measurements R taking into account the extraction matrix H (No real idea how this works haha). This seems to be the covariance for vector y which is just the difference between the predicted measurement and actual measurement. S is factored in to compute the optimal Kalman gain K which, as I’ve said earlier, contains information on how much weight to place on the current prediction x and current observed measurement z that will result the final fused updated state vector x and process covariance matrix P.
This K is the weight given to vector y. The intuition for the Kalman gain K here, as said earlier, is that the state is updated based on a weighted average of the predicted state and the state based on the current measurement, a lower weight is given to that with a higher uncertainty.
In this article, I gave an overview of the Kalman Filter algorithm and what the vectors and matrices mean.
In part 2, I will actually finish implementing the fusion of lidar and radar measurements with this algorithm. I’ll be touching on the Jacobian to estimate a linear extraction matrix H for a non-linear sensor like a radar, how to get the matrix noise covarianceQ from acceleration noise parameters.
🐳 ☕ ️🧧
33Mudy961bUk9zz35p68g9fE3uuHLRduRp